

#include "nav_dwb_critics/twirling.hpp"
#include "pluginlib/class_list_macros.hpp"

namespace nav_dwb_critics
{
void TwirlingCritic::onInit()
{
  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }
  // Scale is set to 0 by default, so if it was not set otherwise, set to 0
  node->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_);
}

double TwirlingCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
{
  return fabs(traj.velocity.theta);  // add cost for making the robot spin
}
}  // namespace nav_dwb_critics

PLUGINLIB_EXPORT_CLASS(nav_dwb_critics::TwirlingCritic, nav_dwb_controller_pkg::TrajectoryCritic)
